'' Octopilot -- PIDController.spin
'' (c) 2009 Ben Levitt
'' http://code.google.com/p/octopilot/
''                                            
'' A single-axis PID controller.
'' Uses Proportional control (a response proportinal to the error -- the difference between the target output and the current value)
''  and Integral control (a response propoprtional to the sum of the error over time)
''  and Derivative control (a response proportional to the current rate of change in the error).


OBJ
  math  :  "FloatMath"         


VAR
  long Kp, Ki, Kd
  long minOutput
  long maxOutput
          
  long integral
  long maxIntegral
  
  long lastCnt                  ' cnt at last GetPosition
  long lastTarget


PUB Start(_Kp, _Ki, _Kd, _minOutput, _maxOutput)
  Kp := _Kp
  Ki := _Ki
  Kd := _Kd
  minOutput  := _minOutput
  maxOutput  := _maxOutput
  maxIntegral := (maxOutput - minOutput)/120  
  Reset


PUB UpdateConstants(_Kp, _Ki, _Kd)
  Kp := _Kp
  Ki := _Ki
  Kd := _Kd


PUB Reset
  integral := 0.0
  lastCnt := 0
  lastTarget := 0


PUB GetOutput(value, rate, target) | time, dT, output, error, tmp
  time := cnt
  
  if ||(target - lastTarget) > 20
    target := lastTarget+(target-lastTarget) / 10
  
  error := math.FFloat(target - value)
  
  if lastCnt == 0
    dT := 0.0
  else
    dT := math.FDiv(math.FFloat(time - lastCnt), math.FFloat(clkfreq))
  integral := math.FAdd(integral, math.FMul(error, dT))
  
  tmp := math.FRound(integral)
  if tmp > maxIntegral
    integral := math.FFloat(maxIntegral)
  elseif tmp < -maxIntegral
    integral := math.FFloat(-maxIntegral)
  
  tmp := math.FFloat(-rate)
  rate := tmp
  
  output := math.FMul(Kp, error)
  tmp := math.FAdd(output, math.FMul(Ki, integral))
  output := tmp
  tmp := math.FAdd(output, math.FMul(Kd, rate))
  output := -math.FRound(tmp)
  
  if output < minOutput
    output := minOutput
  elseif output > maxOutput
    output := maxOutput
  
  lastCnt := time
  lastTarget := target
  
  return output
 